
#include "grasper.h"
#include "math.h"
void grasper::setArmString(string armName)
{
	remote = "/icub/cartesianController/" + armName;
	local  = "/cartesian_client/" + armName;
}

bool grasper::updateModule()
{
	yarp::os::Bottle* cmd = _portGetComm.read();
	
	double x = cmd->get(0).asDouble();
	double y = cmd->get(1).asDouble();
	double z = cmd->get(2).asDouble();
	int comm = cmd->get(3).asDouble();
	
	yarp::os::Bottle& btout = _portGraspComm.prepare();
	btout.clear();
	
	xd[0] = x;         
        xd[1] = y;
        xd[2] = z;
                                 
        //cout << "coordinates sent: (x,y,z) = (" << xd[0] << "," << xd[1] << "," << xd[2] << ")" << endl;
     
        od[0]=0.0; od[1]= 0; od[2]=0; od[3]= M_PI;
	
	if(comm == 0)
	{
        	
        	arm->goToPoseSync(xd,od);
        	char* s = "gs";
        	btout.addString(s);
        	_portGraspComm.write();
	}
	else
	{
		char* s = "oh";
		btout.addString(s);
		_portGraspComm.write();
		for (int i = 0; i < 10000; i++) ;// cout << "waitin to open hand" << endl;;
		arm->goToPoseSync(xd,od);
	}
	
	return true;
}

grasper::grasper()
{
	
}

double 	grasper::getPeriod()
{
	return 0.5;
}

bool 	grasper::close()
{
	return true;
}

bool 	grasper::interruptModule()
{
	 arm->restoreContext(startup_context_id);
	_portGraspComm.interrupt();
	_portMoveArm.interrupt();
	_portGetComm.interrupt();
	return true;
}

bool 	grasper::respond(const yarp::os::Bottle &command,yarp::os::Bottle &reply)
{
	return true;
}

bool	grasper::configure(yarp::os::ResourceFinder &rf)
{
	int x = 0;
	char **argv;
	
	_portGraspComm.open(getName("/o:graspComm"));
	_portGetComm.open(getName("/i:grasper"));
	setArmString("right_arm");
	
	Property option("(device cartesiancontrollerclient)");
        option.put("remote","/icub/cartesianController/right_arm");
        option.put("local","/cartesian_client/right_arm");
        //option.put("remote",remote.c_str());
        //option.put("local",local.c_str());

        //client=new PolyDriver;
        if (!client.open(option))
        {
            cout << "s1" << endl;
            //delete client;    
            return false;
        }
	cout << "s2" << endl;
        // open the view
        client.view(arm);

        // latch the controller context in order to preserve
        // it after closing the module
        // the context contains the dofs status, the tracking mode,
        // the resting positions, the limits and so on.
        arm->storeContext(&startup_context_id);

        // set trajectory time
        arm->setTrajTime(5.0);

        // get the torso dofs
        Vector newDof, curDof;
        arm->getDOF(curDof);
        newDof=curDof;

        // enable the torso yaw and pitch
        // disable the torso roll
        newDof[0]=1;
        newDof[1]=0;
        newDof[2]=1;

        // send the request for dofs reconfiguration
        arm->setDOF(newDof,curDof);

        xd.resize(3);
        od.resize(4);

	int axis=0; // pitch joint
        double min, max;

        // sometimes it may be helpful to reduce
        // the range of variability of the joints;
        // for example here we don't want the torso
        // to lean out more than 30 degrees forward

        // we keep the lower limit
        //arm->getLimits(axis,&min,&max);
        //arm->setLimits(axis,min,MAX_TORSO_PITCH);
	
	
	return true;
}
